#include <stdlib.h>
#include "esp_log.h"
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "freertos/queue.h"
#include "freertos/semphr.h"
#include "myusb.h"
#include "driver/gpio.h"
#include "espFFB.h"
#include "encoder.h"
#include "ParaConfig.h"
#include "USBdesc.h"
#include "esp_timer.h"
#include "Serial.h"
#include "led_strip.h"
#include "MathHelper.h"
#include "nvs_flash.h"  
#include "Motor.h"
#include "Wheel.h"
#include "SystemConfig.h"
#include "JoystickReport.h"
#include "nvs.h"
#include "ParameterManager.h"
#define DEBUG 0
static const char *TAG = "Main";

// 任务句柄
TaskHandle_t xUSBReportTaskHandle = NULL;
TaskHandle_t xUSBCommandTaskHandle = NULL;
TaskHandle_t xForceCalcTaskHandle = NULL;
TaskHandle_t xMotorCtrlTaskHandle = NULL;
TaskHandle_t xEncoderTaskHandle = NULL;

// 通信队列
QueueHandle_t xForceOutputQueue;     // 力输出到电机
QueueHandle_t xEncoderDataQueue;     // 编码器数据
QueueHandle_t xUsbCommandQueue;      // USB命令到命令处理任务
QueueHandle_t xJoystickReportQueue;  // 游戏杆报告队列

// 全局变量
volatile uint32_t g_control_time_ms = 0;

// 使用你提供的游戏杆报告结构
JoystickReport g_joystick_report = {0};

// 设备状态
typedef struct {
    float temperature;      // 温度 °C
    float battery_level;    // 电池电量 %
    bool usb_connected;     // USB连接状态
    uint32_t report_count;  // 报告计数
    uint32_t command_count; // 命令计数
} DeviceStatus_t;

DeviceStatus_t g_device_status = {
    .temperature = 25.0f,
    .battery_level = 100.0f,
    .usb_connected = false,
    .report_count = 0,
    .command_count = 0
};
/****************USB回调的处理************************ */

void tud_hid_set_report_cb(uint8_t instance, uint8_t report_id, hid_report_type_t report_type, uint8_t const* buffer, uint16_t bufsize)
{
    switch (report_type) {
        case HID_REPORT_TYPE_FEATURE:
            if(report_id == CREATE_NEW_EFFECT_REPORT) {
                CreateNewEffect(report_id, buffer, bufsize);
            } else if(report_id == CONFIG_REPORT_ID) {
                //Config(buffer, bufsize);
            }
            break;
      case HID_REPORT_TYPE_OUTPUT:
{

        if (bufsize > 0) {
            // 静态分配缓冲区，避免动态内存分配
            static UsbCommand_t usb_cmd;
            usb_cmd.length = bufsize;
            memcpy(usb_cmd.buffer, buffer, bufsize);
            
            // 使用非阻塞发送，如果队列满则丢弃旧数据
            if (xQueueSend(xUsbCommandQueue, &usb_cmd, 0) != pdTRUE) {
                // 队列满时的处理：可以记录日志或统计丢弃数量
                static uint32_t drop_count = 0;
                drop_count++;
            }
        } 
        break;
}
        default:
            break;
    }
}

void tud_hid_mount_cb(uint8_t instance)
{
    ESP_LOGI(TAG, "USB HID Device Mounted");
    g_device_status.usb_connected = true;
}

void tud_hid_umount_cb(uint8_t instance)
{
    ESP_LOGI(TAG, "USB HID Device Unmounted");
    g_device_status.usb_connected = false;
}

uint8_t const *tud_hid_descriptor_report_cb(uint8_t instance)
{
    return ffb_hid_report_descriptor;
}

uint16_t tud_hid_get_report_cb(uint8_t instance, uint8_t report_id, hid_report_type_t report_type, uint8_t* buffer, uint16_t reqlen)
{
    switch (report_id) {
        case EFFECT_LOAD_STATE_REPORT:
            if(report_type == HID_REPORT_TYPE_FEATURE) {
                memcpy(buffer, &_pidBlockLoadReport, sizeof(_pidBlockLoadReport));
                return sizeof(_pidBlockLoadReport);
            }
            break;
        case PID_POOL_REPORT:
            if(report_type == HID_REPORT_TYPE_FEATURE) {
                FreeAllBlocks();
                uint8_t PID_POOL_REPORT_ARRAY[5] = {120, 0, 0, 5, 0b10000000};
                memcpy(buffer, &PID_POOL_REPORT_ARRAY, sizeof(PID_POOL_REPORT_ARRAY));
                return sizeof(PID_POOL_REPORT_ARRAY);
            }
            break;
    }
    return 0;
}
/************* FreeRTOS任务实现 - 分离的USB任务 ***************/
void send_joystick_report(JoystickReport *report);
// USB报告发送任务 - 定时发送摇杆数据
void vUSBReportTask(void *pvParameters)
{
    ESP_LOGI(TAG, "USB Report Task Started");
    
    JoystickReport joystick_report;
    TickType_t xLastWakeTime = xTaskGetTickCount();
    const TickType_t xReportPeriod = pdMS_TO_TICKS(4); // 250Hz，每4ms发送一次
    
    uint32_t report_interval_counter = 0;
    uint32_t last_status_time = 0;
    
    for(;;) {
        // 严格定时执行，确保250Hz报告频率
        vTaskDelayUntil(&xLastWakeTime, xReportPeriod);
        
        // 检查USB连接状态
        g_device_status.usb_connected = tud_connected();
        
        if(g_device_status.usb_connected) {
            // 从队列获取最新的游戏杆报告（非阻塞）
            if(xQueueReceive(xJoystickReportQueue, &joystick_report, 0) == pdTRUE) {
                // 发送游戏杆报告
                if(tud_hid_ready()) {
                    send_joystick_report(&joystick_report);
                    g_device_status.report_count++;
                    
                    #ifdef DEBUG_USB_REPORT
                    if(report_interval_counter % 62 == 0) { // 约每秒打印一次 (250/4≈62)
                        ESP_LOGI(TAG, "Report Sent - X: %d, Buttons: 0x%02X", 
                                joystick_report.axes.X, joystick_report.buttons[0]);
                    }
                    #endif
                }
            } else {
                // 队列中没有新数据，可以发送上次的数据或生成默认报告
                #ifdef DEBUG_USB_REPORT
                ESP_LOGW(TAG, "No new report data available");
                #endif
            }
        }
        
        // 报告发送统计（每5秒）
        report_interval_counter++;
        uint32_t current_time = xTaskGetTickCount();
        if(current_time - last_status_time >= pdMS_TO_TICKS(5000)) {
            #ifdef DEBUG_USB_STATS
            ESP_LOGI(TAG, "USB Report Stats - Sent: %lu, Rate: %.1fHz", 
                    g_device_status.report_count, 
                    (float)g_device_status.report_count / 5.0f);
            #endif
            g_device_status.report_count = 0;
            last_status_time = current_time;
        }
    }
}

// USB命令处理任务 - 专门处理下发的力反馈命令
void vUSBCommandTask(void *pvParameters)
{
    ESP_LOGI(TAG, "USB Command Task Started");
    
    UsbCommand_t usb_cmd;
    uint32_t command_interval_counter = 0;
    uint32_t last_status_time = 0;
    
    for(;;) {
        // 等待USB命令（可以阻塞等待，因为命令处理不是时间敏感的）
        if(xQueueReceive(xUsbCommandQueue, &usb_cmd, portMAX_DELAY) == pdTRUE) {
            // 处理USB命令
            HandleOutput(usb_cmd.buffer, usb_cmd.length);
            g_device_status.command_count++;
            
            #ifdef DEBUG_USB_COMMAND
            ESP_LOGI(TAG, "USB Command Processed - ReportID: 0x%02X, Length: %d", 
                    usb_cmd.buffer[0], usb_cmd.length);
            #endif
            
            // 命令处理统计（每5秒）
            command_interval_counter++;
            uint32_t current_time = xTaskGetTickCount();
            if(current_time - last_status_time >= pdMS_TO_TICKS(5000)) {
                #ifdef DEBUG_USB_STATS
                ESP_LOGI(TAG, "USB Command Stats - Processed: %lu", g_device_status.command_count);
                #endif
                g_device_status.command_count = 0;
                last_status_time = current_time;
            }
        }
    }
}

// 力反馈计算任务 - 最高优先级
void vForceCalculationTask(void *pvParameters)
{
    ESP_LOGI(TAG, "Force Calculation Task Started");
    
    TickType_t xLastWakeTime = xTaskGetTickCount();
    const TickType_t xCalculationPeriod = 1; // 1ms周期
    
    EncoderData_t encoder_data;
    uint32_t report_update_counter = 0;
    
    for(;;) {
        // 严格定时执行
        vTaskDelayUntil(&xLastWakeTime, xCalculationPeriod);
        
        // 计算力反馈输出
        Output(&EffectPlayingManager, xForceOutputQueue);

    }
}

// 电机控制任务
void vMotorControlTask(void *pvParameters)
{
    ESP_LOGI(TAG, "Motor Control Task Started");
    
    TickType_t xLastWakeTime = xTaskGetTickCount();
    const TickType_t xControlPeriod = 1; // 1ms控制周期
    
    ForceOutput_t force_value;
    
    for(;;) {
        vTaskDelayUntil(&xLastWakeTime, xControlPeriod);
        
        if(xQueueReceive(xForceOutputQueue, &force_value.X, 0) == pdTRUE) {
            //printf("f: %ld\n",force_value.X);
            // float motor_angle = GetMotorPos(0) * 360.0f;
            // float torque = ApplyWheelAngleLimit(motor_angle, force_value.X / 255.0f / 2.0f);
            // SetMotorTorque(0, -torque);
            
            #ifdef DEBUG_OUTPUT
            static uint32_t debug_counter = 0;
            if(debug_counter++ >= 100) { // 每100ms输出一次
                ESP_LOGI(TAG, "Pos: %.2f°, Vel: %.2f, Torque: %.4f", 
                         motor_angle, GetMotorSpeed(0), torque);
                debug_counter = 0;
            }
            #endif
        } else {
            SetMotorTorque(0, 0.0f);
        }
    }
}
int32_t read_encoder_position(){
    //TODO：
    return 0;
}
// 编码器读取任务
void vEncoderReadTask(void *pvParameters)
{
    ESP_LOGI(TAG, "Encoder Read Task Started");
    
    TickType_t xLastWakeTime = xTaskGetTickCount();
    const TickType_t xEncoderPeriod = 1; // 1ms采样周期
    
    EncoderData_t encoder_data;
    int32_t last_position = 0;
    uint32_t last_time = 0;
    
    for(;;) {
        vTaskDelayUntil(&xLastWakeTime, xEncoderPeriod);
        
        // 读取编码器数据
        int32_t current_position = read_encoder_position();
        uint32_t current_time = g_control_time_ms;
        
        // 计算速度（单位：度/秒）
        float dt = (current_time - last_time) / 1000.0f;
        if(dt > 0 && last_time != 0) {
            float position_diff = (current_position - last_position);
            encoder_data.velocity = position_diff / dt;
        } else {
            encoder_data.velocity = 0.0f;
        }
        
        encoder_data.position = current_position;
        encoder_data.timestamp = current_time;
        
        // 发送到队列
        xQueueOverwrite(xEncoderDataQueue, &encoder_data);
        
        last_position = current_position;
        last_time = current_time;
    }
}

/************* 游戏杆报告处理 ***************/
void update_button_states(void);
// 更新游戏杆报告
void update_joystick_report(EncoderData_t *encoder_data, uint32_t timestamp)
{

    // 更新按钮状态
    update_button_states();
}

// // 计算当前所有活跃效果的总强度
// float calculate_total_force_intensity(void)
// {
//     float total_force = 0.0f;
    
//     for(int i = 0; i < MAX_EFFECTS_NUM; i++) {
//         Playing_t *effect = &EffectPlayingManager.PlayList[i];
//         if(effect->PlayState == PLAYING && effect->index > 0) {
//             // 获取效果的当前强度
//             int32_t force_magnitude = GetEffectForce(effect, 0);
//             total_force += fabsf(force_magnitude / 255.0f);
//         }
//     }
    
//     return fminf(total_force, 10.0f); // 限制最大强度
// }

// 发送游戏杆HID报告
void send_joystick_report(JoystickReport *report)
{
    if(!tud_connected()) {
        return;
    }
    
    if(tud_hid_ready()) {
        // 直接发送整个结构体，确保字节对齐和打包正确
        tud_hid_report(0, (uint8_t*)report, sizeof(JoystickReport));
        
        #ifdef DEBUG_HID_REPORT
        static uint32_t last_report_time = 0;
        if(g_control_time_ms - last_report_time > 1000) {
            ESP_LOGI(TAG, "HID Report - X: %d, Y: %d, Buttons: 0x%02X", 
                     report->axes.X, report->axes.Y, report->buttons[0]);
            last_report_time = g_control_time_ms;
        }
        #endif
    }
}
void read_physical_buttons(void);
// 更新按钮状态
void update_button_states(void)
{
    // 清零按钮状态
    memset(g_joystick_report.buttons, 0, sizeof(g_joystick_report.buttons));
    
    // 1. 读取物理按钮
    read_physical_buttons();
    
    // // 2. 基于方向盘位置创建虚拟按钮
    // float steering_pos = g_joystick_report.axes.X / 32767.0f;
    
    // // 左转极限按钮
    // if(steering_pos < -0.9f) {
    //     g_joystick_report.buttons[0] |= (1 << 0); // Button 1
    // }
    
    // // 右转极限按钮
    // if(steering_pos > 0.9f) {
    //     g_joystick_report.buttons[0] |= (1 << 1); // Button 2
    // }
    
    // // 居中状态按钮
    // if(fabsf(steering_pos) < 0.05f) {
    //     g_joystick_report.buttons[0] |= (1 << 2); // Button 3
    // }
    
    // // 3. 基于力反馈强度创建虚拟按钮
    // float force_intensity = calculate_total_force_intensity();
    // if(force_intensity > 0.1f) {
    //     g_joystick_report.buttons[0] |= (1 << 3); // Button 4: 有反馈
    // }
    // if(force_intensity > 2.0f) {
    //     g_joystick_report.buttons[0] |= (1 << 4); // Button 5: 强反馈
    // }
    
    // // 4. 基于设备状态创建虚拟按钮
    // if(g_device_status.temperature > 60.0f) {
    //     g_joystick_report.buttons[0] |= (1 << 5); // Button 6: 高温警告
    // }
    
    // if(g_device_status.battery_level < 20.0f) {
    //     g_joystick_report.buttons[0] |= (1 << 6); // Button 7: 低电量
    // }
    
    // if(!g_device_status.usb_connected) {
    //     g_joystick_report.buttons[0] |= (1 << 7); // Button 8: USB断开
    // }
}

// 读取物理按钮状态
void read_physical_buttons(void)
{
    // 这里需要根据实际硬件连接配置GPIO
    // 示例：假设有4个物理按钮
    /*
    if(gpio_get_level(GPIO_NUM_0)) 
        g_joystick_report.buttons[1] |= (1 << 0); // Button 9
    if(gpio_get_level(GPIO_NUM_1)) 
        g_joystick_report.buttons[1] |= (1 << 1); // Button 10
    if(gpio_get_level(GPIO_NUM_2)) 
        g_joystick_report.buttons[1] |= (1 << 2); // Button 11
    if(gpio_get_level(GPIO_NUM_3)) 
        g_joystick_report.buttons[1] |= (1 << 3); // Button 12
    */
}


// 按钮和编码器初始化
void InputDevicesInit(void)
{
    // 配置按钮GPIO（根据实际硬件调整）
    /*
    gpio_config_t io_conf = {
        .pin_bit_mask = (1ULL << GPIO_NUM_0) | (1ULL << GPIO_NUM_1) | 
                        (1ULL << GPIO_NUM_2) | (1ULL << GPIO_NUM_3),
        .mode = GPIO_MODE_INPUT,
        .pull_up_en = GPIO_PULLUP_ENABLE,
        .pull_down_en = GPIO_PULLDOWN_DISABLE,
        .intr_type = GPIO_INTR_DISABLE,
    };
    gpio_config(&io_conf);
    */
}

/************* 系统监控任务 ***************/

// 系统状态监控任务
void vSystemMonitorTask(void *pvParameters)
{
    ESP_LOGI(TAG, "System Monitor Task Started");
    
    uint32_t last_heartbeat = 0;
    const TickType_t xMonitorPeriod = pdMS_TO_TICKS(1000); // 1秒周期
    
    for(;;) {
        vTaskDelay(xMonitorPeriod);
        
        // 系统心跳和信息输出
        ESP_LOGI(TAG, "System Status - Time: %lus, USB: %s, Battery: %.1f%%, Temp: %.1f°C", 
                 g_control_time_ms / 1000,
                 g_device_status.usb_connected ? "Connected" : "Disconnected",
                 g_device_status.battery_level,
                 g_device_status.temperature);
        
        // 检查力反馈效果状态
        uint8_t active_effects = 0;
        for(int i = 0; i < MAX_EFFECTS_NUM; i++) {
            if(EffectPlayingManager.PlayList[i].PlayState == PLAYING) {
                active_effects++;
            }
        }
        
        if(active_effects > 0) {
            ESP_LOGI(TAG, "Active Effects: %d", active_effects);
        }
        
        // 检查队列状态
        #ifdef DEBUG_QUEUE_STATUS
        UBaseType_t encoder_queue_count = uxQueueMessagesWaiting(xEncoderDataQueue);
        UBaseType_t report_queue_count = uxQueueMessagesWaiting(xJoystickReportQueue);
        UBaseType_t command_queue_count = uxQueueMessagesWaiting(xUsbCommandQueue);
        
        ESP_LOGI(TAG, "Queue Status - Encoder: %d, Report: %d, Command: %d",
                 encoder_queue_count, report_queue_count, command_queue_count);
        #endif
        
        last_heartbeat = g_control_time_ms;
    }
}

/************* 主程序 ***************/

void print_detailed_memory_info() {
    printf("=== Detailed Memory Info ===\n");
    printf("DRAM total: %d bytes\n", heap_caps_get_total_size(MALLOC_CAP_8BIT));
    printf("DRAM free: %d bytes\n", heap_caps_get_free_size(MALLOC_CAP_8BIT));
    printf("IRAM total: %d bytes\n", heap_caps_get_total_size(MALLOC_CAP_32BIT));
    printf("IRAM free: %d bytes\n", heap_caps_get_free_size(MALLOC_CAP_32BIT));
    printf("DMA total: %d bytes\n", heap_caps_get_total_size(MALLOC_CAP_DMA));
    printf("DMA free: %d bytes\n", heap_caps_get_free_size(MALLOC_CAP_DMA));

    // 获取多堆信息
    multi_heap_info_t info;
    heap_caps_get_info(&info, MALLOC_CAP_8BIT);
    printf("DRAM: free=%d, allocated=%d, largest_free_block=%d\n", info.total_free_bytes, info.total_allocated_bytes, info.largest_free_block);

    heap_caps_get_info(&info, MALLOC_CAP_32BIT);
    printf("IRAM: free=%d, allocated=%d, largest_free_block=%d\n", info.total_free_bytes, info.total_allocated_bytes, info.largest_free_block);
}
void app_main(void)
{


    // 创建通信队列
    xForceOutputQueue = xQueueCreate(1, sizeof(ForceOutput_t));      // 覆盖队列
    xEncoderDataQueue = xQueueCreate(1, sizeof(EncoderData_t));      // 覆盖队列  
    xUsbCommandQueue = xQueueCreate(10, sizeof(UsbCommand_t));       // 缓冲队列
    xJoystickReportQueue = xQueueCreate(1, sizeof(JoystickReport));  // 覆盖队列
    
    if(xForceOutputQueue == NULL || xEncoderDataQueue == NULL || 
       xUsbCommandQueue == NULL || xJoystickReportQueue == NULL) {
        ESP_LOGE(TAG, "Failed to create queues!");
        return;
    }
    //初始化参数管理器
    init_parameters_manager();
    //加载所有的参数
    load_all_parameters();
    // printf("g_ParaList",g_ParaList.)


    // 初始化硬件
    InitUSB();
    
    Serial_init();
    InitFFB();              // 初始化力反馈系统
    //Motor_Init();           // 初始化电机
    //Encoder_Init();         // 初始化编码器
    //InputDevicesInit();     // 初始化按钮
    
    // 初始化游戏杆报告
    memset(&g_joystick_report, 0, sizeof(JoystickReport));
    
    //ESP_LOGI(TAG, "Hardware initialization completed");
    
    //创建FreeRTOS任务（按优先级从高到低）
    xTaskCreatePinnedToCore(vForceCalculationTask, "ForceCalc", 4096, NULL, 5, &xForceCalcTaskHandle, 1);
    xTaskCreatePinnedToCore(vMotorControlTask, "MotorCtrl", 4096, NULL, 4, &xMotorCtrlTaskHandle, 1);
    xTaskCreatePinnedToCore(vEncoderReadTask, "EncoderRead", 4096, NULL, 3, &xEncoderTaskHandle, 1);
    //xTaskCreatePinnedToCore(vUSBReportTask, "USB_Report", 4096, NULL, 3, &xUSBReportTaskHandle, 0); // 与编码器同优先级
    xTaskCreatePinnedToCore(vUSBCommandTask, "USB_Cmd", 4096, NULL, 2, &xUSBCommandTaskHandle, 0);
    // xTaskCreatePinnedToCore(vSystemMonitorTask, "Monitor", 4096, NULL, 1, NULL, 0);
    
    ESP_LOGI(TAG, "All tasks created successfully");
    ESP_LOGI(TAG, "Force Feedback Joystick System Ready!");
    // 主循环 - 空闲任务
    
    while (1) {
         vTaskDelay(pdMS_TO_TICKS(1000));
         #if DEBUG
         print_detailed_memory_info();
         #endif
        // esp_err_t e = safe_cdc_send((const uint8_t*)message, strlen(message));
        // if(e==ESP_OK){
        //     printf("send ok\n");
        // }
        // else{
        //     printf("send fail %d\n",e);
        // }
    }
}